//VERSION 1.0 - Flown on Mini Bexus 26. Sept 2012 to ~10km alt

/// @file	vx_var.h
/// @brief	Vexredus Variables

#ifndef vxvar_h
#define vxvar_h

// ########### Defines begin ##################################

// ########### Defines begin ##################################

// ########### Defines begin ##################################

#define FP_MANUAL 0
#define FP_FBW 1
#define FP_NAV1	2
#define FP_NAV2 3
#define FP_DIVE 4
#define FP_TRANSITION 5

#define VX_TELE_PIDLIST_FBW 0
#define VX_TELE_PIDLIST_NAV1_STAB 1
#define VX_TELE_PIDLIST_NAV1_NAV 2
#define VX_TELE_PIDLIST_NAV2_STAB 3
#define VX_TELE_PIDLIST_NAV2_NAV 4
#define VX_TELE_PIDLIST_DIVE_STAB 5
#define VX_TELE_PIDLIST_DIVE_NAV 6
#define VX_TELE_PIDLIST_TRANSITION_STAB 7
#define VX_TELE_PIDLIST_TRANSITION_NAV 8

#define VX_TELE_WARNING_RESTARTED 0
#define VX_TELE_WARNING_CRCERR 1

#define VX_TELE_WARNING_PARACHUTE__BOUNDARYBOX 2
#define VX_TELE_WARNING_PARACHUTE__NOGPSFIX 3
#define VX_TELE_WARNING_PARACHUTE__LOWALTITUDE 4
#define VX_TELE_WARNING_RELEASED 5

#define VX_TELE_WARNING_CANCELED 6
#define VX_TELE_WARNING_CANCELED__NOGPSFIX_GPSFIX 7
#define VX_TELE_WARNING_CANCELED__NOGPSFIX_GS 8
#define VX_TELE_WARNING_CANCELED__NOGPSFIX_RC 9
#define VX_TELE_WARNING_CANCELED__BOUNDARYBOX 10
#define VX_TELE_WARNING_CANCELED__LOWALTITUDE_RC 11
#define VX_TELE_WARNING_CANCELED__LOWALTITUDE_GS 12

#define VX_TELE_WARNING_UAVLAUNCH__MANUAL 13
#define VX_TELE_WARNING_UAVLAUNCH__AUTO_BOX 14
#define VX_TELE_WARNING_UAVLAUNCH__AUTO_DROP 15

#define VX_TELE_WARNING_SMOKER_1 16
#define VX_TELE_WARNING_SMOKER_2 17
#define VX_TELE_WARNING_SMOKER_OFF 18

#define VX_SERIAL 0
#define VX_SERIAL3 1
#define VX_SERIAL_BOTH 2

#define VX_SIM_NONE 0
#define VX_SIM_HIL 1

#define toRad (PI/(180.))
#define RadtoDeg(x) (x*180/PI)
#define GPStoRad(x) (x *PI/18000) //because GPS Groundcourse comes as 100th degree


// ########### Defines end ##################################


// ########### Config begin ##################################

// ### SIMULATION SETTINGS

// VX_SIM: Compiler setting for simulation.
//         VX_SIM__NONE: No simulation activated.
//         VX_SIM__HIL: Hardware-in-the-loop activated, overrides GPS and attitude sensors.

#define VX_SIM VX_SIM_HIL

// ### TELEMETRY AND -COMMAND SETTINGS

// VX_TELECOMMAND_PORT: Serial port to accept telecommands from. (Warning! Do NOT set to VX_SERIAL_BOTH!)
//                      VX_SERIAL: USB port (Serial0)
//                      VX_SERIAL3: XBEE port (Serial3)
#define VX_TELECOMMAND_PORT VX_SERIAL3

// VX_TELEMETRY_PORT: Turn on or off telemetry output on USB port
//                    VX_SERIAL: USB port (Serial0)
//                    VX_SERIAL3: XBEE port (Serial3)
//                    VX_SERIAL_BOTH: Both USB and XBEE ports (Serial0 and Serial3)
#define VX_TELEMETRY_PORT VX_SERIAL3


// NAVIGATION SETTINGS
#define VX_DROP_RECOGNITION_ALTITUDE 10000 // in [cm], altitude difference (from highest recorded to current) to recognize a drop from balloon. -- FLIGHT SETTING

// ########### Config end ##################################

// ########### Types or struct definition begin ##############
struct pidlist_t
{
    float a_1;  //stab: roll p1                           	nav: roll p
    float a_2;  //stab: roll p2                           	nav: roll i
    float a_3;  //stab: roll i1                           	nav: roll d
    float a_4;  //stab: roll i errsum max (anti wind up)  	nav: roll i errsum max
    float a_5;  //stab: roll target rate max				nav: -
    
    
    float b_1;  //stab: nick p1                           	nav: nick p
    float b_2;  //stab: nick p2                           	nav: nick i
    float b_3;  //stab: nick i1                           	nav: nick d
    float b_4;  //stab: nick i errsum max (anti wind up)  	nav: nick i errsum max
    float b_5;  //stab: nick target rate max              	nav: nick target max (max nose up attitude)
    float b_6;  //stab: -                                 	nav: nick target min (max nose down attitude)
};

struct waypoint_t
{
  long lat;      // Lat in degree * 10**7
  long lon;     // Lon in degree * 10**7
  long alt;           // in cm
};

// ########### Types or struct definition end ##############



//###################### VAR begin ###########################

//second setup, only first loop
boolean vx_firstloop = true;
int vx_g_loop_medium_i = 0;
int vx_g_act_case = -1;
//boolean vx_g_act_settime = true;
boolean vx_g_act_case_updated = true;
unsigned long vx_g_act_time = 0;

//PID VARS
pidlist_t vx_g_stab_pidlist_fbw = {6.0,60.0,0.0,0.0,50.0,6.0,60.0,0.0,25.0,50.0,0.0}; // -- FLIGHT SETTING

pidlist_t vx_g_stab_pidlist_nav1 = {6.0,60.0,0.0,0.0,50.0,6.0,60.0,0.0,25.0,50.0,0.0}; // -- FLIGHT SETTING
pidlist_t vx_g_nav_pidlist_nav1 = {0.8,0.0,0.07,0.0,0.2,2.0,0.0,0.0,0.0,0.5000,-0.5000}; // -- FLIGHT SETTING

pidlist_t vx_g_stab_pidlist_nav2 = {6.36,100,0.785,0,100,0,0,0,0,0,0};
pidlist_t vx_g_nav_pidlist_nav2 = {1,0,0,0,0.8,1,0,0,0,0.5,-0.5};

pidlist_t vx_g_stab_pidlist_dive = {0,0,0,0,0,0,0,0,0,0,0};
pidlist_t vx_g_nav_pidlist_dive = {0,0,0,0,0,0,0,0,0,0,0};

pidlist_t vx_g_stab_pidlist_transition = {6.36,100,0.785,0,100,0,0,0,0,0,0};
pidlist_t vx_g_nav_pidlist_transition = {1,0,0,0,0,0,0,0,0,0,0};

pidlist_t * vx_g_stab_pidlistinuse = &vx_g_stab_pidlist_fbw;
int vx_g_pidlistinuse_nr = VX_TELE_PIDLIST_FBW; // Also update this one when changing pidlist

//Stabilize Vars
float vx_error_roll = 0;
float vx_g_target_roll = 0;
float vx_target_rollrate;
float vx_error_rollrate;
float vx_errorsum_rollrate = 0;
int16_t vx_g_stabilizeout_roll = 0;

float vx_error_nick = 0;
float vx_g_target_nick = 0;
float vx_target_nickrate;
float vx_error_nickrate;
float vx_errorsum_nickrate = 0;
int16_t vx_g_stabilizeout_nick = 0;

boolean vx_g_flag_stabilize = false;
int vx_roll_to_elev_mix_value;  //impuls

float vx_scaler_gpsspeed = 1.0;
float vx_g_scaler_tele = 1.0;
float vx_g_speed_scaler = 1.0;

//Servo out
int vx_svout_0 =1500;
int vx_svout_1 =1500;

//Performance measurement
long vx_perf_a_temptime;
long vx_g_perf_a_lastaveragecycletime;
int vx_perf_a_count = 1;


//Flightplan VARS
int vx_g_flightphase = 1;  //
int vx_g_last_autonomous_mode = 100;

//LIMITATION VARS FOR FBW
int16_t vx_max_roll_angle_fbw = 45; //degrees
int16_t vx_max_nick_angle_fbw = 30;

//RCIN VARS
//int vx_g_rcin_0; same as APM_RC.InputCh(0)
uint16_t vx_g_rcin_max_0 = 1900;
uint16_t vx_g_rcin_middle_0 = 1500;
uint16_t vx_g_rcin_min_0 = 1100;
uint16_t vx_g_rcin_diff_max_0 = vx_g_rcin_max_0 - vx_g_rcin_middle_0;
uint16_t vx_g_rcin_diff_min_0 = vx_g_rcin_middle_0 - vx_g_rcin_min_0;

//int vx_g_rcin_1; same as APM_RC.InputCh(1)
uint16_t vx_g_rcin_max_1 =2000;
uint16_t vx_g_rcin_middle_1 = 1500;
uint16_t vx_g_rcin_min_1 = 1000;
uint16_t vx_g_rcin_diff_max_1 = vx_g_rcin_max_1 - vx_g_rcin_middle_1;
uint16_t vx_g_rcin_diff_min_1 = vx_g_rcin_middle_1 - vx_g_rcin_min_1;

//vars for roll
uint16_t vx_g_rcin_max_roll = 2000; //must be read from serial monitor for setup
int16_t vx_g_rcin_middle_roll = 0; //relative value gets set by: vx_g_setmiddledeltawing() !neeeds to be signed!!!! not unsigned
uint16_t vx_g_rcin_min_roll = 1000; //must be read from serial monitor for setup
uint16_t vx_g_rcin_diff_min_roll;// = vx_g_rcin_middle_roll + 1500 - vx_g_rcin_min_roll;
uint16_t vx_g_rcin_diff_max_roll;// = vx_g_rcin_max_roll - vx_g_rcin_middle_roll + 1500;

//vars for nick
int vx_g_rcin_max_nick = 2000; //must be read from serial monitor for setup
int vx_g_rcin_middle_nick = 0; //relative value gets set by: vx_g_setmiddledeltawing() !neeeds to be signed!!!! not unsigned
int vx_g_rcin_min_nick = 1000; //must be read from serial monitor for setup
int vx_g_rcin_diff_min_nick;// = vx_g_rcin_middle_nick + 1500 - vx_g_rcin_min_nick;
int vx_g_rcin_diff_max_nick;// = vx_g_rcin_max_nick - vx_g_rcin_middle_nick + 1500;

int vx_g_transmitter_flightphase = 1; //Flightphase set by transmitter
int vx_g_transmitter_flightphase_last = 1; //Last Flightphase set by transmitter


//oscillation checker
int vx_g_osci_max_value = 0;
int vx_g_osci_min_value = 0;
long vx_g_osci_time = 0;

//vars for navigation
//waypoint_t vx_g_home = {678917000, 210817000, 70000}; // On ESRANGE
waypoint_t vx_g_home = {678880656,210848543, 66000}; // BALLOON PAD -- FLIGHT SETTING
waypoint_t vx_g_wpin = {678880656,210848543, 66000};
waypoint_t vx_g_wp_guided = {678880656,210848543, 66000};

waypoint_t vx_g_center_circle = {680935217, 210410983, 66000}; // CENTRE OF CIRCLE (PARACHUTE & LAUNCH) -- FLIGHT SETTING

boolean vx_wp_reached = false;
  
float vx_nav_course_heading;
float vx_g_target_heading;
float vx_nav_diff_heading;
float vx_nav_error_heading;
float vx_nav_error_heading_old = 0;
float vx_nav_errorrate_heading = 0;
float vx_target_flightpathangle;
float vx_nav_error_nick;
float vx_nav_error_nick_old;
float vx_nav_errorrate_nick;


long vx_lat1 = 0; //[deg * 01^7]
long vx_lat2 = 0;//[deg * 01^7]
float vx_dlat;//[deg * 01^7]
long vx_lon1 = 0;//[deg * 01^7]
long vx_lon2 = 0;//[deg * 01^7]
float vx_dlon;//[deg * 01^7]
long vx_alt1 = 0;//[cm]
long vx_alt2 = 0;//[cm]
float vx_dalt;

long vx_g_wpdistance; //in [m]
int vx_wpradius = 250; //distance to reach wp [m] -- FLIGHT SETTING

float scaleLongDown = 1;

long vx_g_baro_alt; //[cm]
long vx_p0 =101325; //[Pa]
int vx_altitude_correction = 0; //[m]

pidlist_t * vx_g_nav_pidlistinuse;

//NAv damping
//boolean vx_g_calculate_navdampening = true;


//use fixed nick angle (flight path angle)
boolean vx_g_use_fixed_flightpathangle = false;
float vx_g_fixed_flightpathangle = 0;

//use fixed heading angle
boolean vx_g_use_fixed_target_heading = false;
long vx_g_use_fixed_target_heading_timer = 0;
float vx_g_fixed_target_heading = 0;

// Telemetry global variables
long vx_g_tele_last_received_time = 0; // Last time UAV received a message from Ground Station.

// Positioning global variables
waypoint_t vx_g_current_pos = {0,0,0}; // Current position (ACHTUNG: ALWAYS READ FROM THIS, NOT g_gps!)
long vx_g_current_ground_speed = 0; // ground speed in cm/sec
long vx_g_current_ground_course = 0; // ground course in 100ths of a degree (from north?)

boolean vx_g_pos_had_gps_fix = false; // Ever had GPS fix
long vx_g_pos_last_gps_fix_time = 0; // Last time GPS had fix.

long vx_g_highest_altitude = 0; // in [cm], Highest altitude recorded in flight (since restart)

// Parachute global variables
long vx_g_para_countdown_clock = -1; // Parachute auto release countdown clock (initially set to show no current countdown: -1)
boolean vx_g_para_ascent = true; // For ascent 

// Launch (drop from balloon) servo global variables
boolean vx_g_launch_servo = false;


//GS FBW
int vx_g_gsfbw_nick = 0;
int vx_g_gsfbw_roll = 0;

int vx_gsfbw_nick_last = 1;
int vx_gsfbw_roll_last = 1;
long vx_gsfbw_time = 0;
boolean vx_gsfbw_called = false;

//Neutral Case (7)
boolean vx_neutralrudders = false;
long vx_neutralrudders_timer = 0;


//Smoker

int vx_g_smoker_state = 0;







//HIL
long vx_g_hil_lat = 0; //GPS Latitude [degree * 10^7, long integer]
long vx_g_hil_lon = 0; //GPS Longitude [degree * 10^7, long integer]
long vx_g_hil_alt = 0; //GPS MSL Altitude [m * 100, integer]
int vx_g_hil_hdg = 0;// GPS Heading [degrees * 100, integer]
int vx_g_hil_gpsspd = 0;// GPS Speed (ground) [m/s*100, integer]

float vx_g_hil_roll = 0; //Attitude [rad]
float vx_g_hil_nick = 0; //Attitude [rad]



String vx_HIL_sendBuffer; // Send buffer
String vx_HIL_recvBuffer; // Receive buffer
char vx_HIL_recv_c; // Receive char
bool vx_HIL_recv_buffering = false;// Buffering logic
String vx_HIL_recvMsgHeader;
String vx_HIL_crc_s;

int vx_hil_svout_case = 0;







//###################### VAR end ###########################






//interfaces still to move here or to consider for use on other autopilot platforms:

//ahrs.roll(); ahrs.nick()
//millis()
//APM_RC.OutputCh
//APM_RC.InputCh

//serial.print





#endif
